#!/usr/bin/python2
#coding=utf-8

"""
INSTALLATION INSTRUCTIONS:
0. Install dependencies:
        sudo apt install python-rosbag 
1. Create a symlink in ~/bin to this script so you can run it from anywhere:
        cd /path/to/here
        mkdir -p ~/bin
        chmod +x ${PWD}/read_bag.py 
        ln -si "${PWD}/read_bag.py" ~/bin/read_bag
2. Now you can use the `read_bag` command directly anywhere you like.
"""

import rosbag
import os,sys

def printHelp():
    print("Usage: {} <xxx.bag> [topic1 ...] | tee output.txt".format(os.path.basename(sys.argv[0])))
    sys.exit()

class Args: # 命令行参数
    bag_file_in = ""
    topics_to_read = None # None在rosbag中默认读取所有topic
    print_all_msg = False

def parseArgs():
    args = Args()
    if len(sys.argv) == 1:
        print("ERROR: no input args; 1 minimum required.")
        printHelp()
        
    if len(sys.argv) >= 2:
        if sys.argv[1] == "-h" or sys.argv[1] == "--help":
            printHelp()
        if sys.argv[1] == "-p" or sys.argv[1] == "--print":
            # TO DO: 这里的2是一个隐藏的BUG
            args.bag_file_in = sys.argv[2]
        else:
            args.bag_file_in = sys.argv[1] 
    
    if len(sys.argv) >= 3:
        if sys.argv[1] == "-p" or sys.argv[1] == "--print":
            args.print_all_msg = True
            args.topics_to_read = sys.argv[3:]
        else:
            args.topics_to_read = sys.argv[2:]
    return args

def readBag(args):
    # msg_counters[topic]中存放的是各个topic的当前消息总数,初始是一个空的dict
    msg_counters = {}
    total_count = 0
    bag_in = rosbag.Bag(args.bag_file_in)
    
    for topic, msg, t in bag_in.read_messages(args.topics_to_read):
        if msg_counters.has_key(topic) == False:
            msg_counters[topic] = 1
        else:
            msg_counters[topic] += 1
        total_count += 1

        # 先打印整个消息，看看数据结构
        if args.print_all_msg == True:
            print(msg) 
            print("================================")
	    else:    
            # aloam & hdl
            pos = msg.pose.pose.position
            ori = msg.pose.pose.orientation
                         

            # ndt_mapping
            #pos = msg.pose.position
            #ori = msg.pose.orientation

            # 志勇record的是/aft_mapped_path, 消息是累积的路径poses, 因此需要取最后一个元素
            # pos = msg.poses[msg_counters[topic]-1].pose.position
	    # ori = msg.poses[msg_counters[topic]-1].pose.orientation
            print("{0} {1} {2} {3} {4} {5} {6} {7}".format(msg_counters[topic], pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w))
        
    if total_count == 0:
        print("NO MESSAGES FOUND IN THESE TOPICS")

if __name__ == '__main__':
    args = parseArgs()
    readBag(args)
